/*---------------*/
//    1 } - { 4  //
//        I      //
//	  2 } - { 3  //
/*---------------*/
#include <PS2X_lib.h>      //手柄控制
#include <stdlib.h>
#include <freertos/message_buffer.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h> 
#include "DeviceManager.h" 

const char *ssid = "albert"; // AP模式的WIFI热点名称
const char *password = "88888888";// WIFI密码   ** 注意这里只能连接2.4G的wifi
const char* mqtt_server = "192.168.43.92";  

int port = 1884; 
const char* mqtt_user = "admin"; // 如果需要的话
const char* mqtt_password = "admin"; // 如果需要的话
 
MessageBufferHandle_t xMessageBuffer = NULL;
DeviceManager deviceManager;
 
void ps2xTask(void * pvParam) { //ps2手柄配置
  byte vibrate = 0;
  byte type = 0; 
  int error = -1;
  int tryNum = 1;  
  
  while (error != 0) {
    vTaskDelay(1000/portTICK_PERIOD_MS);
    //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    error = deviceManager.ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);
    Serial.print("#try config ");
    Serial.println(tryNum);
    tryNum ++;  
  } 
  Serial.println(deviceManager.ps2x.Analog(1), HEX);
  type = deviceManager.ps2x.readType();
  switch(type) {
    case 0: Serial.println(" Unknown Controller type found "); break;
    case 1: Serial.println(" DualShock Controller found "); break;
    case 2: Serial.println(" GuitarHero Controller found "); break;
    case 3: Serial.println(" Wireless Sony DualShock Controller found "); break;
  }
  if(type != 1){ 
    return;
  }
  size_t xBytesSent; //The number of bytes written to the message buffer.
  PS2XData ps2Data;
  while (1) {//循环读取ps2的状态 
     deviceManager.ps2x.read_gamepad(false, vibrate); 
     vibrate = deviceManager.ps2x.Analog(PSAB_CROSS);
     bool PSB_L1_press = deviceManager.ps2x.Button(PSB_L1); 
     bool PSB_L2_press = deviceManager.ps2x.Button(PSB_L2);
     bool PSB_R1_press = deviceManager.ps2x.Button(PSB_R1); 
     bool PSB_R2_press = deviceManager.ps2x.Button(PSB_R2);
     int l_x = deviceManager.ps2x.Analog(PSS_LX);
     int l_y = deviceManager.ps2x.Analog(PSS_LY);  

     l_x = l_x <= 127 ? -(127 - l_x) : l_x - 127;
     l_y = l_y <= 128 ? (128 - l_y) : -(l_y - 128); 
     ps2Data.radius = 127;
     ps2Data.x = l_x;
     ps2Data.y = l_y; 
     ps2Data.leftRotation = PSB_L1_press;
     ps2Data.leftSlide = PSB_L2_press;
     ps2Data.rightRotation = PSB_R1_press;
     ps2Data.rightSlide = PSB_R2_press;
      
     xBytesSent = xMessageBufferSend(xMessageBuffer,
                                   (void *)&ps2Data,
                                   sizeof(ps2Data),
                                   portMAX_DELAY);
    if (xBytesSent != sizeof(ps2Data)){
      Serial.println("危险: xMessageBufferSend 发送数据不完整");
    }
    vTaskDelay(50/portTICK_PERIOD_MS);  
  }  
  vTaskDelete(NULL);
}
  
//操作小车
void carTask(void * pvParam){
  size_t xReceivedBytes;
  const size_t xMessageSizeMax = 100;
  PS2XData ps2Data;
  while (1) {  
    xReceivedBytes = xMessageBufferReceive(xMessageBuffer,
                                          (void *)&ps2Data,
                                          xMessageSizeMax, //This sets the maximum length of the message that can be received.
                                          portMAX_DELAY);
    if (xReceivedBytes > 0){
      deviceManager.car.startCar(ps2Data.leftRotation, ps2Data.leftSlide, ps2Data.rightRotation, ps2Data.rightSlide, ps2Data.radius, ps2Data.x, ps2Data.y);
    }
    vTaskDelay(10/portTICK_PERIOD_MS); 
  }  
  vTaskDelete(NULL);
}

// WiFi 和 MQTT 客户端
WiFiClient espClient;
PubSubClient mqttClient(espClient);
// 子线程任务句柄
TaskHandle_t mqttTaskHandle = NULL;
TaskHandle_t ps2TaskHandle = NULL;

void setup() {
  Serial.begin(115200); 
  const size_t xMessageBufferSizeBytes = sizeof(PS2XData) * 20;
  xMessageBuffer = xMessageBufferCreate(xMessageBufferSizeBytes);

  if (xMessageBuffer == NULL){
    Serial.println("Unable to Create Message Buffer");
    esp_restart(); 
  } else {
    deviceManager = DeviceManager(); 
    xTaskCreatePinnedToCore(ps2xTask, "配置ps2", 4096, NULL, 1, &ps2TaskHandle, 0);  
    xTaskCreatePinnedToCore(carTask, "控制小车", 1024 * 10, NULL, 1, NULL, 0);
    xTaskCreatePinnedToCore(mqttTask, "MQTT_Service", 1024 * 10, NULL, 2, &mqttTaskHandle, 1);
    enablePS2(false);
  } 
  vTaskDelete(NULL);
}
 
// 子线程任务（MQTT服务）
void mqttTask(void* pvParameters) {  
  mqttClient.setBufferSize(1024); // 设置缓冲区大小，根据需要调整
  mqttClient.setKeepAlive(10); // 设置心跳间隔，单位为秒
  mqttClient.setCallback(callback);
  mqttClient.setServer(mqtt_server, port); 
 for (;;) {
    // 尝试连接WiFi（非阻塞）
    if (!connectWiFi()) {
      vTaskDelay(1000 / portTICK_PERIOD_MS);
      continue;
    }
    if (mqttClient.connected() == true) {
      mqttClient.loop();
    } else {
      connectMQTT();
    }
    vTaskDelay(10 / portTICK_PERIOD_MS); // 用 FreeRTOS 延时，避免占满CPU
  }
}

// WiFi 非阻塞连接
bool connectWiFi() { 
  if (WiFi.status() == WL_CONNECTED) return true;
  
  Serial.printf("连接 WiFi %s...\n", ssid);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);  //开始连接wifi
  unsigned long start = millis();
  while (millis() - start < 3000) { // 最多等3秒
   if (WiFi.status() == WL_CONNECTED) {
     Serial.printf("\nWiFi 已连接, IP地址: %s\n", WiFi.localIP().toString().c_str());
     return true;
   }
   vTaskDelay(100 / portTICK_PERIOD_MS);
  }
  return false;
}

// MQTT 连接函数
void connectMQTT() {
  if (mqttClient.connected()) return;
 
  Serial.print("连接 MQTT...");
  String clientId = "esp32-" + WiFi.macAddress();
  if (mqttClient.connect(clientId.c_str())) { 
    Serial.printf("连接成功, rc=%d", mqttClient.state());
    //连接成功后就订阅主题
    mqttClient.subscribe("BoardReceiveMsg_aaa");//订阅主题 
 
  } else { 
    Serial.printf("连接失败, rc=%d, 0.5秒后重试\n", mqttClient.state());
    vTaskDelay(500 / portTICK_PERIOD_MS); // 用 FreeRTOS 延时，避免占满CPU
  }
}
void callback(char* topic, byte* payload, unsigned int length) {
  // 将payload转换为字符串
  String receivedStr = "";
  for (int i = 0; i < length; i++) {
    receivedStr += (char)payload[i];
  } 
  Serial.println(receivedStr);
  
  StaticJsonDocument<1024> receivedDict; //创建一个足够大的DynamicJsonDocument对象  
  deserializeJson(receivedDict, receivedStr); //反序列化数据 将String转换为JsonVariant
 
  int code = receivedDict["code"].as<int>();
  int messageType = receivedDict["messageType"].as<int>();
  if (code != 200) {
    return;
  }

  if (messageType == 1) {
    StaticJsonDocument<512> receivedData = receivedDict["data"];
    bool leftRotation = receivedData["leftRotation"].as<bool>();
    bool leftSlide = receivedData["leftSlide"].as<bool>();
    bool rightRotation = receivedData["rightRotation"].as<bool>();
    bool rightSlide = receivedData["rightSlide"].as<bool>();
    float percentX = receivedData["percentX"].as<float>();
    float percentY = receivedData["percentY"].as<float>();
   
    size_t xBytesSent; //The number of bytes written to the message buffer.
    PS2XData ps2Data;
    ps2Data.radius = 127;
    ps2Data.x = percentX * 127.0f;
    ps2Data.y = percentY * 127.0f; 
    ps2Data.leftRotation = leftRotation;
    ps2Data.leftSlide = leftSlide;
    ps2Data.rightRotation = rightRotation;
    ps2Data.rightSlide = rightSlide;

    xBytesSent = xMessageBufferSend(xMessageBuffer,
                                  (void *)&ps2Data,
                                  sizeof(ps2Data),
                                  portMAX_DELAY);
    if (xBytesSent != sizeof(ps2Data)){
      Serial.println("危险: xMessageBufferSend 发送数据不完整");
    }
  } else if (messageType == 2) {//控制摄像头

  } else if (messageType == 3) {//控制机械臂

  } else if(messageType == 4) {//挂起或恢复ps2手柄控制
    StaticJsonDocument<512> receivedData = receivedDict["data"];
    bool enable = receivedData["enablePS2"].as<bool>();
    enablePS2(enable);
  } else {
    JsonArray data = receivedDict["data"];
    for (size_t i = 0; i < data.size(); i++) {
      JsonObject itemDict = data[i];
    }
  } 

  receivedDict.clear();
}

void enablePS2(bool enable) {
    if (ps2TaskHandle == NULL) {
       return;
    } 
    if(enable == true) {
      vTaskResume(ps2TaskHandle); // 恢复 PS2 任务
    } else { 
      vTaskSuspend(ps2TaskHandle);// 挂起 PS2 任务
    }
      deviceManager.car.startCar(false, false, false, false, 127, 0, 0);
}

void loop() {   
   
}